#include "MathTools.h"

#include <Eigen/Core>
#include <Eigen/Geometry>

using std::cout;
using std::endl;

namespace MMM
{
namespace Math
{


	Eigen::Matrix4f rpy2eigen4f (float r, float p, float y)
	{
		float salpha,calpha,sbeta,cbeta,sgamma,cgamma;

		sgamma = sinf(r);
		cgamma = cosf(r);
		sbeta = sinf(p);
		cbeta = cosf(p);
		salpha = sinf(y);
		calpha = cosf(y);

		Eigen::Matrix4f m;

		m(0,0) = (float)(calpha*cbeta);
		m(0,1) = (float)(calpha*sbeta*sgamma-salpha*cgamma);
		m(0,2) = (float)(calpha*sbeta*cgamma+salpha*sgamma);
		m(0,3) = 0;//x

		m(1,0) = (float)(salpha*cbeta);
		m(1,1) = (float)(salpha*sbeta*sgamma+calpha*cgamma);
		m(1,2) = (float)(salpha*sbeta*cgamma-calpha*sgamma);
		m(1,3) = 0;//y

		m(2,0) = (float)-sbeta;
		m(2,1) = (float)(cbeta*sgamma);
		m(2,2) = (float)(cbeta*cgamma);
		m(2,3) = 0;//z

		m(3,0) = 0;
		m(3,1) = 0;
		m(3,2) = 0;
		m(3,3) = 1.0f;

		return m;
	}

	Eigen::Matrix4f quat2eigen4f( float x, float y, float z, float w )
	{
		Eigen::Matrix4f m;
		m.setIdentity();
		Eigen::Quaternionf q(w,x,y,z);
		Eigen::Matrix3f m3;
		m3 = q.toRotationMatrix();
		m.block(0,0,3,3) = m3;
		return m;
	}

	Eigen::Vector3f matrix3fToEulerXZY(const Eigen::Matrix3f &m)
	{
		Eigen::Vector3f rotEuler = m.eulerAngles(0, 2, 1);
		return rotEuler;
	}

	Eigen::Vector3f matrix3fToRPY(const Eigen::Matrix3f &m)
	{
		//Eigen::Matrix4f A = mat.transpose();
		float alpha, beta, gamma;
		beta = atan2(-m(2, 0), sqrtf(m(0, 0)*m(0, 0) + m(1, 0)*m(1, 0)));
		if (fabs(beta - (float)M_PI*0.5f) < 1e-10)
		{
			alpha = 0;
			gamma = atan2(m(0, 1), m(1, 1));
		}
		else if (fabs(beta + (float)M_PI*0.5f) < 1e-10)
		{
			alpha = 0;
			gamma = -atan2(m(0, 1), m(1, 1));
		}
		else
		{
			float cb = 1.0f / cosf(beta);
			alpha = atan2(m(1, 0)*cb, m(0, 0)*cb);
			gamma = atan2(m(2, 1)*cb, m(2, 2)*cb);
		}
		Eigen::Vector3f res(gamma, beta, alpha);
		return res;
	}

	Eigen::Vector3f matrix4fToEulerXZY( const Eigen::Matrix4f &m) 
	{
		return matrix3fToEulerXZY(m.block(0,0,3,3));
	}

	Eigen::VectorXf matrix4fToPoseEulerXZY(const Eigen::Matrix4f &m)
	{
		Eigen::VectorXf r(6);
		r.head(3) = m.block(0, 3, 3, 1);
		r.tail(3) = matrix3fToEulerXZY(m.block(0, 0, 3, 3));
		return r;
	}

	Eigen::VectorXf matrix4fToPoseRPY(const Eigen::Matrix4f &m)
	{
		Eigen::VectorXf r(6);
		r.head(3) = m.block(0, 3, 3, 1);
		r.tail(3) = matrix3fToRPY(m.block(0, 0, 3, 3));
		return r;
	}

	Eigen::Matrix3f eulerXZYToMatrix3f( const Eigen::Vector3f &eulerXZY )
	{
		Eigen::Matrix3f m_3;
		m_3 =  Eigen::AngleAxisf(eulerXZY[0], Eigen::Vector3f::UnitX())
			* Eigen::AngleAxisf(eulerXZY[1], Eigen::Vector3f::UnitZ())
			* Eigen::AngleAxisf(eulerXZY[2], Eigen::Vector3f::UnitY());
		return m_3;
	}

	Eigen::Matrix4f poseEulerXZYToMatrix4f( const Eigen::Vector3f &pos, const Eigen::Vector3f &eulerXZY )
	{
		Eigen::Matrix4f m;
		m.setIdentity();
		m.block(0,3,3,1) = pos;

		m.block(0,0,3,3) = eulerXZYToMatrix3f(eulerXZY);
		return m;
	}

	Eigen::Matrix4f poseRPYToMatrix4f( const Eigen::Vector3f &pos, const Eigen::Vector3f &rpy )
	{
		Eigen::Matrix4f m;
		m.setIdentity();
		m = rpy2eigen4f(rpy(0),rpy(1),rpy(2));
		m.block(0,3,3,1) = pos;
		return m;
	}

    Eigen::Vector3f linearInterpolation(const Eigen::Vector3f &value1, float v1_timestep, const Eigen::Vector3f &value2, float v2_timestep, float timestep) {
        Eigen::Vector3f interpolatedVector;
        for (int i = 0; i < 3; i++) {
            interpolatedVector(i) = Math::linearInterpolation(value1(i), v1_timestep, value2(i), v2_timestep, timestep);
        }
        return interpolatedVector;
    }

    float linearInterpolation(float value1, float v1_timestep, float value2, float v2_timestep, float timestep) {
        return value1 + (value2 - value1) / (v2_timestep - v1_timestep) * (timestep - v1_timestep);
    }

    float roundf(float f, uint8_t precision) {
        unsigned long help = std::pow(10, precision);
        return std::floor((f * help + 0.5f)) / help;
    }

    bool equal(float f1, float f2) {
        return abs(f1 - f2) < 0.000001;
    }

#ifdef ALGLIB_AVAILABLE
    SplineFunction::SplineFunction(Eigen::VectorXf input) {
        int len_input = input.rows();

        this->derivative = Eigen::VectorXf::Zero(len_input);
        this->derivative2ndOrder = Eigen::VectorXf::Zero(len_input);
        alglib::real_1d_array data_part,x_points_part;
        alglib::ae_int_t info;
        alglib::spline1dfitreport rep;
        alglib::spline1dinterpolant s;
        int window = len_input;
        for(int j = 0;j< len_input; j++) {

            data_part.setlength(window);
            x_points_part.setlength(window);

            for(int i = 0; i< window; i++){
                data_part(i) = input(i)*100;  // inits data and scales data to 100Hz
                x_points_part(i) = i;
            }
        }

        double rho = 0.0; //-0.5
        alglib::spline1dfitpenalized(x_points_part,data_part,80,rho,info,s,rep); // 7

        for(int i = 0; i< len_input; i++){
            double der,f, der2;
            alglib::spline1ddiff(s,double(i),f,der,der2);
            derivative(i)  = der; // +0.2;
            derivative2ndOrder(i) = der2*100;
        }
    }
#endif
}

}
